#include <ros/ros.h>
#include "TransPoint.hpp" //相机标定插件

#include <ar_track_alvar_msgs/AlvarMarkers.h> //ar码消息类型
#include "arm_controller/move.h" //手臂服务消息类型
#include <cv_eyehand/markers_pixel.h>
#include <cv_eyehand/marker_pixel.h>
#include <cv_eyehand/transpoint.h>

#include <opencv2/opencv.hpp> //opencv库
#include <sensor_msgs/CameraInfo.h> // 相机参数消息类型
#include <sensor_msgs/Image.h> // ROS图像消息类型
#include <sensor_msgs/CameraInfo.h> //相机参数文件
#include <cv_bridge/cv_bridge.h> //OpenCV和ROS图像消息转换库

#include <ros/package.h>

#include <geometry_msgs/Vector3.h>
#include <iostream>
#include <string>
#include <list>

using namespace cv;
using namespace std;

class ar_track_pixel
{
public:
    ar_track_pixel();
    void pixel(double x,double y,double z);
    bool goto_pos(double x,double y,double z);
    bool tf_callbacksrv(cv_eyehand::transpoint::Request& req,cv_eyehand::transpoint::Response& res);
    void mkcallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg); //订阅ar码
    void imgcallback(const sensor_msgs::Image::ConstPtr& msg); //订阅相机画面
    void infocallback(const sensor_msgs::CameraInfo::ConstPtr& msg);//订阅相机参数
    Mat frame; //消息文件
    Mat P; //相机参数（投影矩阵)
    double marker_x,marker_y; //像素坐标
    bool sw;
    string filedir = ros::package::getPath("cv_eyehand") + "/dat/matrix.dat";
private:
    ros::NodeHandle n;
    ros::Publisher pix_pub;
    ros::Subscriber mk_sub; //订阅ar码消息
    ros::Subscriber img_sub; //订阅相机画面消息
    ros::Subscriber info_sub; //订阅相机参数消息
    ros::ServiceServer tf_srv; //像素坐标转机器人坐标服务
    ros::ServiceClient goto_client = n.serviceClient<arm_controller::move>("goto_position"); //定义"goto_position"客户端    
};

ar_track_pixel::ar_track_pixel(){
    info_sub = n.subscribe<sensor_msgs::CameraInfo>("/hand_cam/usb_cam/camera_info",10,&ar_track_pixel::infocallback,this);
    mk_sub = n.subscribe<ar_track_alvar_msgs::AlvarMarkers>("/hand_cam/ar_pose_marker", 10, &ar_track_pixel::mkcallback,this);
    img_sub = n.subscribe<sensor_msgs::Image>("/hand_cam/usb_cam/image_raw",10,&ar_track_pixel::imgcallback,this); 
    pix_pub = n.advertise<cv_eyehand::markers_pixel>("/ar_track_pixel",10,this); //发布ar码像素
    tf_srv = n.advertiseService("/transpoint",&ar_track_pixel::tf_callbacksrv,this);
    ros::spin();
}

void ar_track_pixel::infocallback(const sensor_msgs::CameraInfo::ConstPtr& msg){
    //获取相机投影参数
    Mat cam_Info(3,4,CV_64F);
    int num = 0;
    for(int i=0;i<3;i++){
        for(int j=0;j<4;j++){
            cam_Info.ptr<double>(i)[j] = msg->P[num];
            num++;
        }
    }
    P = cam_Info;
}

void ar_track_pixel::mkcallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg){
    //转换ar像素坐标
    cv_eyehand::marker_pixel pix;
    cv_eyehand::markers_pixel mks_pixel;
    
    for(int i=0;i<msg->markers.size();i++){
        double pose_x = msg->markers[i].pose.pose.position.x*10;
        double pose_y = msg->markers[i].pose.pose.position.y*10;
        double pose_z = msg->markers[i].pose.pose.position.z*10;
        int ID = msg->markers[i].id;
        double cam_pos[4] = {pose_x, pose_y,pose_z,1};
        Mat T_cam(4, 1, CV_64F, cam_pos);
        Mat pixel_pos = P * T_cam/pose_z;
        marker_x = (double)pixel_pos.at<double>(0,0);
        marker_y = (double)pixel_pos.at<double>(0,1);
        cv::circle(frame,cv::Point(marker_x,marker_y),4,Scalar(0,0,255),8);
        const string& text = "ID_" + to_string(ID);
        cv::putText(frame,text,cv::Point(marker_x+5,marker_y+5),FONT_HERSHEY_SIMPLEX,1, Scalar(255,255,255), 4);
        pix.id = ID;
        pix.pixel.x = (double)pixel_pos.at<double>(0,0);
        pix.pixel.y = (double)pixel_pos.at<double>(0,1);
        mks_pixel.markers.push_back(pix);
    }
    imshow("Image", frame);
    pix_pub.publish(mks_pixel);
    waitKey(30);
}

void ar_track_pixel::imgcallback(const sensor_msgs::Image::ConstPtr& msg){
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    frame = cv_ptr -> image;
}

bool ar_track_pixel::tf_callbacksrv(cv_eyehand::transpoint::Request& req,cv_eyehand::transpoint::Response& res){
    Homat2D* mat;
    Homat2D mat1;
    Point2F point, point1;
    mat = &mat1;
    bool results = loadMatrix(mat,filedir);
    point.x=req.x;
    point.y=req.y;
    point1 = affineTrans2DPoint(mat1, point);
    res.x = point1.x;
    res.y = point1.y;
    res.result = true;
}

bool ar_track_pixel::goto_pos(double x,double y,double z){
    ros::service::waitForService("goto_position"); //等待“goto_position”服务开启
    arm_controller::move srv; // 创建服务消息
    srv.request.pose.position.x = x;
    srv.request.pose.position.y = y;
    srv.request.pose.position.z = z;
    goto_client.call(srv); //发送请求
    return srv.response.success;
}

int main(int argc, char **argv){
    ros::init(argc, argv, "ar_track_pixel"); //初始化节点
    ar_track_pixel cv_eh;
    return 0;    
}




